function [x, f, eflag, output, lambda] = mips(f_fcn, x0, A, l, u, xmin, xmax, gh_fcn, hess_fcn, opt)
% mips - MATPOWER Interior Point Solver (|MIPS>|).
% ::
%
%   [x, f, exitflag, output, lambda] = ...
%       mips(f_fcn, x0, A, l, u, xmin, xmax, gh_fcn, hess_fcn, opt);
%
%   x = mips(f_fcn, x0)
%   x = mips(f_fcn, x0, A, l)
%   x = mips(f_fcn, x0, A, l, u)
%   x = mips(f_fcn, x0, A, l, u, xmin)
%   x = mips(f_fcn, x0, A, l, u, xmin, xmax)
%   x = mips(f_fcn, x0, A, l, u, xmin, xmax, gh_fcn, hess_fcn)
%   x = mips(f_fcn, x0, A, l, u, xmin, xmax, gh_fcn, hess_fcn, opt)
%   x = mips(problem)
%           where problem is a struct with fields:
%               f_fcn, x0, A, l, u, xmin, xmax, gh_fcn, hess_fcn, opt
%               all fields except f_fcn and x0 are optional
%   x = mips(...)
%   [x, f] = mips(...)
%   [x, f, exitflag] = mips(...)
%   [x, f, exitflag, output] = mips(...)
%   [x, f, exitflag, output, lambda] = mips(...)
%
% Primal-dual interior point method for NLP (nonlinear programming).
% Minimize a function :math:`f(\x)` beginning from a starting point
% ``x0``, subject to optional linear and nonlinear constraints and
% variable bounds.
%
% .. math:: \min_\x f(\x)
%   :label: eq_mips_obj_fcn
%
% subject to
%
% .. math:: \g(\x) = 0
% .. math:: \h(\x) \le 0
% .. math:: \param{\rvec{l}} \le \param{\rmat{A}} \x \le \param{\rvec{u}}
%   :label: eq_mips_lin_constraint
% .. math:: \param{\x}_\mathrm{min} \le \x \le \param{\x}_\mathrm{max}
%   :label: eq_mips_var_bounds
%
% where :eq:`eq_mips_obj_fcn`--:eq:`eq_mips_var_bounds` are,
% respectively, the objective function, nonlinear equality constraints,
% nonlinear inequality constraints, linear constraints, and variable bounds.
%
% Inputs:
%   : *(all optional except* ``f_fcn`` *and* ``x0`` *)*
%   f_fcn (function handle) : handle to function that evaluates the
%       objective function :math:`f(\x)`, its gradients and Hessian for
%       a given value of :math:`x`. If there are nonlinear constraints,
%       the Hessian information is provided by the ``hess_fcn`` function
%       passed in the 9th argument and is not required here. Calling
%       syntax for this function::
%
%           [f, df, d2f] = f_fcn(x)
%
%   x0 (double) : starting value of optimization vector :math:`\x`
%   A, l, u (double) : *(optional, respective defaults*
%       :math:`[empty, -\infty, +\infty]` *)* matrix :math:`\param{\cmat{A}}`
%       and vectors :math:`\param{\rvec{l}}` and :math:`\param{\rvec{u}}`
%       to define the linear constraints in :eq:`eq_mips_lin_constraint`
%   xmin, xmax (double) : *(optional, respective defaults*
%       :math:`[-\infty, +\infty]` *)*  lower and upper bounds,
%       :math:`\param{\x}_\mathrm{min}` and :math:`\param{\x}_\mathrm{max}`,
%       on the optimization variables :math:`\x`
%   gh_fcn (function handle) : *(optional)* handle to function that
%       evaluates the nonlinear constraints :math:`\g(\x)` and :math:`\h(\x)`
%       and their gradients for a given value of :math:`x`. Calling syntax
%       for this function is::
%
%           [h, g, dh, dg] = gh_fcn(x)
%
%       where the columns of ``dh`` and ``dg`` are the gradients of the
%       corresponding elements of ``h`` and ``g``, i.e. ``dh`` and ``dg``
%       are transposes of the Jacobians of ``h`` and ``g``, respectively.
%   hess_fcn (function handle) : *(optional)* handle to function that
%       computes the Hessian of the Lagrangian for given values of
%       :math:`\x`, :math:`\lam` and :math:`\muv`, where
%       :math:`\lam` and :math:`\muv` are the multipliers on the equality
%       and inequality constraints, :math:`\g` and :math:`\h`, respectively.
%       The calling syntax for this function is::
%
%           Lxx = hess_fcn(x, lam)
%
%       where :math:`\lam` = ``lam.eqnonlin`` and :math:`\muv` =
%       ``lam.ineqnonlin``.
%   opt (struct) : *(optional)* options structure with the following fields,
%       all of which are also optional *(default values shown in
%       parentheses)*
%
%       - ``verbose`` *(0)* - controls level of progress output displayed
%
%           - 0 = no progress output
%           - 1 = some progress output
%           - 2 = verbose progress output
%       - ``linsolver`` *(* ``''`` *)* - linear system solver for solving
%         update steps
%
%           - ``''`` = default solver (currently same as ``'\'``)
%           - ``'\'`` = built-in ``\`` operator
%           - ``'PARDISO'`` = PARDISO solver *(if available)*
%       - ``feastol`` *(1e-6)* - termination tolerance for feasibility
%         condition
%       - ``gradtol`` *(1e-6)* - termination tolerance for gradient
%         condition
%       - ``comptol`` *(1e-6)* - termination tolerance for complementarity
%         condition
%       - ``costtol`` *(1e-6)* - termination tolerance for cost condition
%       - ``max_it`` *(150)* - maximum number of iterations
%       - ``step_control`` *(0)* - set to 1 to enable step-size control
%       - ``sc.red_it`` *(20)* - maximum number of step-size reductions if
%         step-control is on
%       - ``cost_mult`` *(1)* - cost multiplier used to scale the objective
%         function for improved conditioning. *Note: This value is
%         also passed as the 3rd argument to the Hessian evaluation
%         function so that it can appropriately scale the
%         objective function term in the Hessian of the
%         Lagrangian.*
%       - ``xi`` *(0.99995)* - constant :math:`\xi` used in :math:`alpha`
%         updates [#]_
%       - ``sigma`` *(0.1)* - centering parameter :math:`\sigma` [1]_
%       - ``z0`` *(1)* - used to initialize slack variables [1]_
%       - ``alpha_min`` *(1e-8)* - returns ``exitflag`` = -1 if either alpha
%         parameter, :math:`\alpha_p` or :math:`\alpha_d` becomes smaller
%         than this value [1]_
%       - ``rho_min`` *(0.95)* - lower bound on ``rho_t`` [1]_
%       - ``rho_max`` *(1.05)* - upper bound on ``rho_t`` [1]_
%       - ``mu_threshold`` *(1e-5)* - KT multipliers smaller than this value
%         for non-binding constraints are forced to zero
%       - ``max_stepsize`` *(1e10)* - returns ``exitflag`` = -1 if the
%         2-norm of the reduced Newton step exceeds this value [1]_
%       - ``full_hist`` *(0)* - set to 1 to enable saving in ``output.hist``
%         trajectories of ``x``, ``z``, ``g``, ``h``, ``lam``, ``mu``
%   problem (struct) : The inputs can alternatively be supplied in a single
%       ``problem`` struct with fields corresponding to the input arguments
%       described above, namely, ``f_fcn``, ``x0``, ``A``, ``l``, ``u``,
%       ``xmin``, ``xmax``, ``gh_fcn``, ``hess_fcn``, and ``opt``, where
%       all except ``f_fcn`` and ``x0`` are optional
%
% .. [#] See the "Primal-Dual Interior Point Algorithm" chapter in the
%    |MIPSman| for details.
%
% Outputs:
%   x (double) : solution vector, :math:`\x`
%   f (double) : final objective function value, :math:`f(\x)`
%   exitflag (integer) : exit flag
%
%       - 1 = first order optimality conditions satisfied
%       - 0 = maximum number of iterations reached
%       - -1 = numerically failed
%   output (struct) : output struct with fields:
%
%       - ``iterations`` - number of iterations performed
%       - ``hist`` - struct array with trajectories of the following:
%
%         - ``feascond``, ``gradcond``, ``compcond``, ``costcond``,
%           ``gamma``, ``stepsize``, ``obj``, ``alphap``, ``alphad``
%       - ``message`` - exit message
%   lambda (struct) : struct containing the Langrange and Kuhn-Tucker
%       multipliers on the constraints, with fields:
%
%       - ``eqnonlin`` - nonlinear equality constraints
%       - ``ineqnonlin`` - nonlinear inequality constraints
%       - ``mu_l`` - lower (left-hand) limit on linear constraints
%       - ``mu_u`` - upper (right-hand) limit on linear constraints
%       - ``lower`` - lower bound on optimization variables
%       - ``upper`` - upper bound on optimization variables
%
% **Note:** The calling syntax is almost identical to that of :func:`fmincon`
% from MathWorks' Optimization Toolbox. The main difference is that
% the linear constraints are specified with ``A``, ``l``, ``u`` instead of
% ``A``, ``B``, ``Aeq``, ``Beq``. The functions for evaluating the objective
% function, constraints and Hessian are identical.
%
% **Example:** (problem from https://en.wikipedia.org/wiki/Nonlinear_programming)::
%
%   function [f, df, d2f] = f2(x)
%   f = -x(1)*x(2) - x(2)*x(3);
%   if nargout > 1           % gradient is required
%       df = -[x(2); x(1)+x(3); x(2)];
%       if nargout > 2       % Hessian is required
%           d2f = -[0 1 0; 1 0 1; 0 1 0];   % actually not used since
%       end                                 % 'hess_fcn' is provided
%   end
%   
%   function [h, g, dh, dg] = gh2(x)
%   h = [ 1 -1 1; 1 1 1] * x.^2 + [-2; -10];
%   dh = 2 * [x(1) x(1); -x(2) x(2); x(3) x(3)];
%   g = []; dg = [];
%   
%   function Lxx = hess2(x, lam, cost_mult)
%   if nargin < 3, cost_mult = 1; end
%   mu = lam.ineqnonlin;
%   Lxx = cost_mult * [0 -1 0; -1 0 -1; 0 -1 0] + ...
%           [2*[1 1]*mu 0 0; 0 2*[-1 1]*mu 0; 0 0 2*[1 1]*mu];
%   
%   problem = struct( ...
%       'f_fcn',    @(x)f2(x), ...
%       'gh_fcn',   @(x)gh2(x), ...
%       'hess_fcn', @(x, lam, cost_mult)hess2(x, lam, cost_mult), ...
%       'x0',       [1; 1; 0], ...
%       'opt',      struct('verbose', 2) ...
%   );
%   [x, f, exitflag, output, lambda] = mips(problem);
%
% Ported by Ray Zimmerman from C code written by H. Wang for his
% PhD dissertation:
%
%   H. Wang, *On the Computation and Application of Multi-period
%   Security-Constrained Optimal Power Flow for Real-time
%   Electricity Market Operations,* Ph.D. thesis, Electrical and
%   Computer Engineering, Cornell University, May 2007.
%
% Please see also:
%
%   H. Wang, C. E. Murillo-Sanchez, R. D. Zimmerman, R. J. Thomas,
%   "On Computational Issues of Market-Based Optimal Power Flow",
%   *IEEE Transactions on Power Systems*, Vol. 22, No. 3, Aug. 2007,
%   pp. 1185-1193. :doi:`10.1109/TPWRS.2007.901301`

%   MIPS
%   Copyright (c) 2009-2024, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MIPS.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mips for more info.

%%----- input argument handling  -----
%% gather inputs
if nargin == 1 && isstruct(f_fcn)       %% problem struct
    p = f_fcn;
    f_fcn = p.f_fcn;
    x0 = p.x0;
    nx = size(x0, 1);       %% number of optimization variables
    if isfield(p, 'opt'),       opt = p.opt;            else,   opt = [];       end
    if isfield(p, 'hess_fcn'),  hess_fcn = p.hess_fcn;  else,   hess_fcn = '';  end
    if isfield(p, 'gh_fcn'),    gh_fcn = p.gh_fcn;      else,   gh_fcn = '';    end
    if isfield(p, 'xmax'),      xmax = p.xmax;          else,   xmax = [];      end
    if isfield(p, 'xmin'),      xmin = p.xmin;          else,   xmin = [];      end
    if isfield(p, 'u'),         u = p.u;                else,   u = [];         end
    if isfield(p, 'l'),         l = p.l;                else,   l = [];         end
    if isfield(p, 'A'),         A = p.A;                else,   A=sparse(0,nx); end
else                                    %% individual args
    nx = size(x0, 1);       %% number of optimization variables
    if nargin < 10
        opt = [];
        if nargin < 9
            hess_fcn = '';
            if nargin < 8
                gh_fcn = '';
                if nargin < 7
                    xmax = [];
                    if nargin < 6
                        xmin = [];
                        if nargin < 5
                            u = [];
                            if nargin < 4
                                l = [];
                                A = sparse(0,nx);
                            end
                        end
                    end
                end
            end
        end
    end
end
%% set default argument values if missing
if isempty(A) || (~isempty(A) && (isempty(l) || all(l == -Inf)) && ...
                                 (isempty(u) || all(u == Inf)))
    A = sparse(0,nx);           %% no limits => no linear constraints
end
nA = size(A, 1);                %% number of original linear constraints
if isempty(u)                   %% By default, linear inequalities are ...
    u = Inf(nA, 1);             %% ... unbounded above and ...
end
if isempty(l)
    l = -Inf(nA, 1);            %% ... unbounded below.
end
if isempty(xmin)                %% By default, optimization variables are ...
    xmin = -Inf(nx, 1);         %% ... unbounded below and ...
end
if isempty(xmax)
    xmax = Inf(nx, 1);          %% ... unbounded above.
end
if isempty(gh_fcn)
    nonlinear = false;          %% no nonlinear constraints present
    gn = []; hn = [];
else
    nonlinear = true;           %% we have some nonlinear constraints
end

%% default options
if isempty(opt)
    opt = struct('verbose', 0);
end
if ~isfield(opt, 'linsolver') || isempty(opt.linsolver)
    opt.linsolver = '';
end
if ~isfield(opt, 'feastol') || isempty(opt.feastol) || opt.feastol == 0
    opt.feastol = 1e-6;
end
if ~isfield(opt, 'gradtol') || isempty(opt.gradtol) || opt.gradtol == 0
    opt.gradtol = 1e-6;
end
if ~isfield(opt, 'comptol') || isempty(opt.comptol) || opt.comptol == 0
    opt.comptol = 1e-6;
end
if ~isfield(opt, 'costtol') || isempty(opt.costtol) || opt.costtol == 0
    opt.costtol = 1e-6;
end
if ~isfield(opt, 'max_it') || isempty(opt.max_it)
    opt.max_it = 150;
end
if ~isfield(opt, 'sc') || ~isfield(opt.sc, 'red_it') || isempty(opt.sc.red_it)
    opt.sc.red_it = 20;
end
if ~isfield(opt, 'step_control') || isempty(opt.step_control)
    opt.step_control = 0;
end
if ~isfield(opt, 'cost_mult') || isempty(opt.cost_mult)
    opt.cost_mult = 1;
end
if ~isfield(opt, 'verbose') || isempty(opt.verbose)
    opt.verbose = 0;
end
%% algorithm constants
if ~isfield(opt, 'xi') || isempty(opt.xi)
    opt.xi = 0.99995;           %% OPT_IPM_PHI
end
if ~isfield(opt, 'sigma') || isempty(opt.sigma)
    opt.sigma = 0.1;            %% OPT_IPM_SIGMA, centering parameter
end
if ~isfield(opt, 'z0') || isempty(opt.z0)
    opt.z0 = 1;                 %% OPT_IPM_INIT_SLACK
end
if ~isfield(opt, 'alpha_min') || isempty(opt.alpha_min)
    opt.alpha_min = 1e-8;       %% OPT_AP_AD_MIN
end
if ~isfield(opt, 'rho_min') || isempty(opt.rho_min)
    opt.rho_min = 0.95;         %% OPT_IPM_QUAD_LOWTHRESH
end
if ~isfield(opt, 'rho_max') || isempty(opt.rho_max)
    opt.rho_max = 1.05;         %% OPT_IPM_QUAD_HIGHTHRESH
end
if ~isfield(opt, 'mu_threshold') || isempty(opt.mu_threshold)
    opt.mu_threshold = 1e-5;    %% SCOPF_MULTIPLIERS_FILTER_THRESH
end
if ~isfield(opt, 'max_stepsize') || isempty(opt.max_stepsize)
    opt.max_stepsize = 1e10;
end
if ~isfield(opt, 'full_hist') || isempty(opt.full_hist)
    opt.full_hist = 0;
end

%% initialize history
if opt.full_hist
    hist(opt.max_it+1) = struct('feascond', 0, 'gradcond', 0, 'compcond', 0, ...
        'costcond', 0, 'gamma', 0, 'stepsize', 0, 'obj', 0, ...
        'alphap', 0, 'alphad', 0, 'x', [], 'z', [], 'g', [], 'h', [], ...
        'lam', [], 'mu', []);
else
    hist(opt.max_it+1) = struct('feascond', 0, 'gradcond', 0, 'compcond', 0, ...
        'costcond', 0, 'gamma', 0, 'stepsize', 0, 'obj', 0, ...
        'alphap', 0, 'alphad', 0);
end

%%-----  set up problem  -----
%% constants
[xi, sigma, z0, alpha_min, rho_min, rho_max, mu_threshold, max_stepsize] = ...
    deal(opt.xi, opt.sigma, opt.z0, opt.alpha_min, ...
        opt.rho_min, opt.rho_max, opt.mu_threshold, opt.max_stepsize);
if xi >= 1 || xi < 0.5
    error('mips: opt.xi (%g) must be a number slightly less than 1', opt.xi);
end
if sigma > 1 || sigma <= 0
    error('mips: opt.sigma (%g) must be a number between 0 and 1', opt.sigma);
end

%% initialize
i = 0;                      %% iteration counter
converged = 0;              %% flag
eflag = 0;                  %% exit flag

%% add var limits to linear constraints
AA = [speye(nx); A];
ll = [xmin; l];
uu = [xmax; u];

%% split up linear constraints
ieq = find( abs(uu-ll) <= eps );            %% equality
igt = find( uu >=  1e10 & ll > -1e10 );     %% greater than, unbounded above
ilt = find( ll <= -1e10 & uu <  1e10 );     %% less than, unbounded below
ibx = find( (abs(uu-ll) > eps) & (uu < 1e10) & (ll > -1e10) );
Ae = AA(ieq, :);
be = uu(ieq, 1);
Ai  = [ AA(ilt, :); -AA(igt, :); AA(ibx, :); -AA(ibx, :) ];
bi  = [ uu(ilt, 1); -ll(igt, 1); uu(ibx, 1); -ll(ibx, 1) ];

%% evaluate cost f(x0) and constraints g(x0), h(x0)
x = x0;
[f, df] = f_fcn(x);             %% cost
f = f * opt.cost_mult;
df = df * opt.cost_mult;
if nonlinear
    [hn, gn, dhn, dgn] = gh_fcn(x); %% nonlinear constraints
    h = [hn; Ai * x - bi];          %% inequality constraints
    g = [gn; Ae * x - be];          %% equality constraints
    dh = [dhn Ai'];                 %% 1st derivative of inequalities
    dg = [dgn Ae'];                 %% 1st derivative of equalities
else
    h = Ai * x - bi;                %% inequality constraints
    g = Ae * x - be;                %% equality constraints
    dh = Ai';                       %% 1st derivative of inequalities
    dg = Ae';                       %% 1st derivative of equalities
end

%% grab some dimensions
neq = size(g, 1);           %% number of equality constraints
niq = size(h, 1);           %% number of inequality constraints
neqnln = size(gn, 1);       %% number of nonlinear equality constraints
niqnln = size(hn, 1);       %% number of nonlinear inequality constraints
nlt = length(ilt);          %% number of upper bounded linear inequalities
ngt = length(igt);          %% number of lower bounded linear inequalities
nbx = length(ibx);          %% number of doubly bounded linear inequalities

%% initialize gamma, lam, mu, z, e
gamma = 1;                  %% barrier coefficient, r in Harry's code
lam = zeros(neq, 1);
z   = z0 * ones(niq, 1);
mu  = z;
k = find(h < -z0);
z(k) = -h(k);
k = find(gamma ./ z > z0);  %% (seems k is always empty if gamma = z0 = 1)
if ~isempty(k)
    mu(k) = gamma / z(k);
end
e = ones(niq, 1);

%% check tolerance
f0 = f;
if opt.step_control
    L = f + lam' * g + mu' * (h+z) - gamma * sum(log(z));
end
Lx = df + dg * lam + dh * mu;
if isempty(h)
    maxh = zeros(1,0);
else
    maxh = max(h);
end
feascond = max([norm(g, Inf), maxh]) / (1 + max([ norm(x, Inf), norm(z, Inf) ]));
gradcond = norm(Lx, Inf) / (1 + max([ norm(lam, Inf), norm(mu, Inf) ]));
compcond = (z' * mu) / (1 + norm(x, Inf));
costcond = abs(f - f0) / (1 + abs(f0));
%% save history
if opt.full_hist
    hist(i+1) = struct('feascond', feascond, 'gradcond', gradcond, ...
        'compcond', compcond, 'costcond', costcond, 'gamma', gamma, ...
        'stepsize', 0, 'obj', f/opt.cost_mult, 'alphap', 0, 'alphad', 0, ...
        'x', x, 'z', z, 'g', g, 'h', h, 'lam', lam, 'mu', mu);
else
    hist(i+1) = struct('feascond', feascond, 'gradcond', gradcond, ...
        'compcond', compcond, 'costcond', costcond, 'gamma', gamma, ...
        'stepsize', 0, 'obj', f/opt.cost_mult, 'alphap', 0, 'alphad', 0);
end
if strcmp(upper(opt.linsolver), 'PARDISO')
    ls = 'PARDISO';
    mplinsolve_opt = struct('pardiso', ...
                            struct('mtype', -2, ...
                                   'iparm', [8, 1;  %% max it refinement steps
                                             10, 12;%% eps pivot
                                             11, 1; %% use scaling vectors
                                             13, 2; %% improved accuracy
                                             18, 0; %% do not determine nnz in LU
                                             21, 3; %% ? undocumented pivoting
                                             ] ));
else
    ls = 'built-in';
    mplinsolve_opt = [];
end
if opt.verbose
    if opt.step_control, s = '-sc'; else, s = ''; end
    v = mipsver('all');
    fprintf('MATPOWER Interior Point Solver -- MIPS%s, Version %s, %s\n (using %s linear solver)', ...
        s, v.Version, v.Date, ls);
    if opt.verbose > 1
        fprintf('\n it    objective   step size   feascond     gradcond     compcond     costcond  ');
        fprintf('\n----  ------------ --------- ------------ ------------ ------------ ------------');
        fprintf('\n%3d  %12.8g %10s %12g %12g %12g %12g', ...
            i, f/opt.cost_mult, '', feascond, gradcond, compcond, costcond);
    end
end
if feascond < opt.feastol && gradcond < opt.gradtol && ...
                compcond < opt.comptol && costcond < opt.costtol
    converged = 1;
    if opt.verbose
        fprintf('\nConverged!\n');
    end
end

%%-----  do Newton iterations  -----
while (~converged && i < opt.max_it)
    %% update iteration counter
    i = i + 1;

    %% compute update step
    lambda = struct('eqnonlin', lam(1:neqnln), 'ineqnonlin', mu(1:niqnln));
    if nonlinear
        if isempty(hess_fcn)
            fprintf('mips: Hessian evaluation via finite differences not yet implemented.\n       Please provide your own hessian evaluation function.');
        end
        Lxx = hess_fcn(x, lambda, opt.cost_mult);
    else
        [f_, df_, d2f] = f_fcn(x);      %% cost
        Lxx = d2f * opt.cost_mult;
    end
    zinvdiag = sparse(1:niq, 1:niq, 1 ./ z, niq, niq);
    mudiag = sparse(1:niq, 1:niq, mu, niq, niq);
    dh_zinv = dh * zinvdiag;
    M = Lxx + dh_zinv * mudiag * dh';
    N = Lx + dh_zinv * (mudiag * h + gamma * e);
    dxdlam = mplinsolve([M dg; dg' sparse(neq, neq)], [-N; -g], opt.linsolver, mplinsolve_opt);
%     AAA = [
%         M  dg;
%         dg'  sparse(neq, neq)
%     ];
%     rc = 1/condest(AAA);
%     if rc < 1e-22
%         fprintf('my RCOND = %g\n', rc);
%         n = size(AAA, 1);
%         AAA = AAA + 1e-3 * speye(n,n);
%     end
%     bbb = [-N; -g];
%     dxdlam = AAA \ bbb;
    if any(isnan(dxdlam)) || norm(dxdlam) > max_stepsize
        if opt.verbose
            fprintf('\nNumerically Failed\n');
        end
        eflag = -1;
        break;
    end
    dx = dxdlam(1:nx, 1);
    dlam = dxdlam(nx+(1:neq), 1);
    dz = -h - z - dh' * dx;
    dmu = -mu + zinvdiag *(gamma*e - mudiag * dz);

    %% optional step-size control
    sc = 0;
    if opt.step_control
        x1 = x + dx;

        %% evaluate cost, constraints, derivatives at x1
        [f1, df1] = f_fcn(x1);          %% cost
        f1 = f1 * opt.cost_mult;
        df1 = df1 * opt.cost_mult;
        if nonlinear
            [hn1, gn1, dhn1, dgn1] = gh_fcn(x1); %% nonlinear constraints
            h1 = [hn1; Ai * x1 - bi];       %% inequality constraints
            g1 = [gn1; Ae * x1 - be];       %% equality constraints
            dh1 = [dhn1 Ai'];               %% 1st derivative of inequalities
            dg1 = [dgn1 Ae'];               %% 1st derivative of equalities
        else
            h1 = Ai * x1 - bi;              %% inequality constraints
            g1 = Ae * x1 - be;              %% equality constraints
            dh1 = dh;                       %% 1st derivative of inequalities
            dg1 = dg;                       %% 1st derivative of equalities
        end

        %% check tolerance
        Lx1 = df1 + dg1 * lam + dh1 * mu;
        if isempty(h1)
            maxh1 = zeros(1,0);
        else
            maxh1 = max(h1);
        end
        feascond1 = max([norm(g1, Inf), maxh1]) / (1 + max([ norm(x1, Inf), norm(z, Inf) ]));
        gradcond1 = norm(Lx1, Inf) / (1 + max([ norm(lam, Inf), norm(mu, Inf) ]));

        if feascond1 > feascond && gradcond1 > gradcond
            sc = 1;
        end
    end
    if sc
        alpha = 1;
        for j = 1:opt.sc.red_it
            dx1 = alpha * dx;
            x1 = x + dx1;
            f1 = f_fcn(x1);                 %% cost
            f1 = f1 * opt.cost_mult;
            if nonlinear
                [hn1, gn1] = gh_fcn(x1);    %% nonlinear constraints
                h1 = [hn1; Ai * x1 - bi];   %% inequality constraints
                g1 = [gn1; Ae * x1 - be];   %% equality constraints
            else
                h1 = Ai * x1 - bi;          %% inequality constraints
                g1 = Ae * x1 - be;          %% equality constraints
            end
            L1 = f1 + lam' * g1 + mu' * (h1+z) - gamma * sum(log(z));
            if opt.verbose > 2
                fprintf('\n   %3d            %10g', -j, norm(dx1));
            end
            rho = (L1 - L) / (Lx' * dx1 + 0.5 * dx1' * Lxx * dx1);
            if rho > rho_min && rho < rho_max
                break;
            else
                alpha = alpha / 2;
            end
        end
        dx = alpha * dx;
        dz = alpha * dz;
        dlam = alpha * dlam;
        dmu = alpha * dmu;
    end

    %% do the update
    k = find(dz < 0);
    if isempty(k)
        alphap = 1;
    else
        alphap = min( [xi * min(z(k) ./ -dz(k)) 1] );
    end
    k = find(dmu < 0);
    if isempty(k)
        alphad = 1;
    else
        alphad = min( [xi * min(mu(k) ./ -dmu(k)) 1] );
    end
    x = x + alphap * dx;
    z = z + alphap * dz;
    lam = lam + alphad * dlam;
    mu  = mu  + alphad * dmu;
    if niq > 0
        gamma = sigma * (z' * mu) / niq;
    end

    %% evaluate cost, constraints, derivatives
    [f, df] = f_fcn(x);                 %% cost
    f = f * opt.cost_mult;
    df = df * opt.cost_mult;
    if nonlinear
        [hn, gn, dhn, dgn] = gh_fcn(x); %% nonlinear constraints
        h = [hn; Ai * x - bi];          %% inequality constraints
        g = [gn; Ae * x - be];          %% equality constraints
        dh = [dhn Ai'];                 %% 1st derivative of inequalities
        dg = [dgn Ae'];                 %% 1st derivative of equalities
    else
        h = Ai * x - bi;                %% inequality constraints
        g = Ae * x - be;                %% equality constraints
        %% 1st derivatives are constant, still dh = Ai', dg = Ae'
    end

    %% check tolerance
    Lx = df + dg * lam + dh * mu;
    if isempty(h)
        maxh = zeros(1,0);
    else
        maxh = max(h);
    end
    feascond = max([norm(g, Inf), maxh]) / (1 + max([ norm(x, Inf), norm(z, Inf) ]));
    gradcond = norm(Lx, Inf) / (1 + max([ norm(lam, Inf), norm(mu, Inf) ]));
    compcond = (z' * mu) / (1 + norm(x, Inf));
    costcond = abs(f - f0) / (1 + abs(f0));
    %% save history
    if opt.full_hist
        hist(i+1) = struct('feascond', feascond, 'gradcond', gradcond, ...
            'compcond', compcond, 'costcond', costcond, 'gamma', gamma, ...
            'stepsize', norm(dx), 'obj', f/opt.cost_mult, ...
            'alphap', alphap, 'alphad', alphad, ...
            'x', x, 'z', z, 'g', g, 'h', h, 'lam', lam, 'mu', mu);
    else
        hist(i+1) = struct('feascond', feascond, 'gradcond', gradcond, ...
            'compcond', compcond, 'costcond', costcond, 'gamma', gamma, ...
            'stepsize', norm(dx), 'obj', f/opt.cost_mult, ...
            'alphap', alphap, 'alphad', alphad);
    end

    if opt.verbose > 1
        fprintf('\n%3d  %12.8g %10.5g %12g %12g %12g %12g', ...
            i, f/opt.cost_mult, norm(dx), feascond, gradcond, compcond, costcond);
    end
    if feascond < opt.feastol && gradcond < opt.gradtol && ...
                    compcond < opt.comptol && costcond < opt.costtol
        converged = 1;
        if opt.verbose
            fprintf('\nConverged!\n');
        end
    else
        if any(isnan(x)) || alphap < alpha_min || alphad < alpha_min || ...
                gamma < eps || gamma > 1/eps
            if opt.verbose
                fprintf('\nNumerically Failed\n');
            end
            eflag = -1;
            break;
        end
        f0 = f;
        if opt.step_control
            L = f + lam' * g + mu' * (h+z) - gamma * sum(log(z));
        end
    end
end

if opt.verbose
    if ~converged
        fprintf('\nDid not converge in %d iterations.\n', i);
    end
end

%%-----  package up results  -----
hist = hist(1:i+1);
if eflag ~= -1
    eflag = converged;
end
output = struct('iterations', i, 'hist', hist, 'message', '');
if eflag == 0
    output.message = 'Did not converge';
elseif eflag == 1
    output.message = 'Converged';
elseif eflag == -1
    output.message = 'Numerically failed';
else
    output.message = 'Please hang up and dial again';
end

%% zero out multipliers on non-binding constraints
mu(h < -opt.feastol & mu < mu_threshold) = 0;

%% un-scale cost and prices
f   = f   / opt.cost_mult;
lam = lam / opt.cost_mult;
mu  = mu  / opt.cost_mult;

%% re-package multipliers into struct
lam_lin = lam((neqnln+1):neq);              %% lambda for linear constraints
mu_lin  = mu((niqnln+1):niq);               %% mu for linear constraints
kl = find(lam_lin < 0);                     %% lower bound binding
ku = find(lam_lin > 0);                     %% upper bound binding

mu_l = zeros(nx+nA, 1);
mu_l(ieq(kl)) = -lam_lin(kl);
mu_l(igt) = mu_lin(nlt+(1:ngt));
mu_l(ibx) = mu_lin(nlt+ngt+nbx+(1:nbx));

mu_u = zeros(nx+nA, 1);
mu_u(ieq(ku)) = lam_lin(ku);
mu_u(ilt) = mu_lin(1:nlt);
mu_u(ibx) = mu_lin(nlt+ngt+(1:nbx));

fields = { ...
    'mu_l', mu_l((nx+1):end), ...
    'mu_u', mu_u((nx+1):end), ...
    'lower', mu_l(1:nx), ...
    'upper', mu_u(1:nx) ...
};

if niqnln > 0
    fields = { ...
        'ineqnonlin', mu(1:niqnln), ...
        fields{:} ...
    };
end
if neqnln > 0
    fields = { ...
        'eqnonlin', lam(1:neqnln), ...
        fields{:} ...
    };
end

lambda = struct(fields{:});

% lambda = struct( ...
%     'eqnonlin', lam(1:neqnln), ...
%     'ineqnonlin', mu(1:niqnln), ...
%     'mu_l', mu_l((nx+1):end), ...
%     'mu_u', mu_u((nx+1):end), ...
%     'lower', mu_l(1:nx), ...
%     'upper', mu_u(1:nx) );
